#!/usr/bin/env python

"""
An echo server that uses select to handle multiple clients at a time.
Entering any line of input at the terminal will exit the server.
"""

import select
import socket
import sys
import serial
import struct

def stp():
	byte0 = int(0xFE)
	byte1 = int(0xFF)
	id1 = int(0x0D)
	id2 = int(0x1E)
	id3 = int(0x19)
	fwd = int(0x06)
	stpB = int(0x00)
	bytef = int(0xEA)
	
	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, id1, fwd, stpB, id2, fwd, stpB, id3, fwd, stpB, bytef)
	ser.write(packet_int)


def usrConnected():
	byte0 = int(0xFE)
	byte1 = int(0xCC)
	dc = int(0x00)
	bytef = int(0xEA)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, dc, dc, dc, dc, dc, dc, dc, dc, dc, bytef)
	ser.write(packet_int)

def usrDisconnected():
	byte0 = int(0xFE)
	byte1 = int(0xDD)
	dc = int(0x00)
	bytef = int(0xEA)

	packet_int = struct.pack('BBBBBBBBBBBB', byte0, byte1, dc, dc, dc, dc, dc, dc, dc, dc, dc, bytef)
	ser.write(packet_int)

ser = serial.Serial()
ser.port = "/dev/ttyUSB0"
#ser.port = "/dev/ttyO2"
ser.baudrate = 38400
ser.timeout = 0
ser.close()		#ensure port is closed
ser.open()

host = ''
port = 50000
backlog = 5
size = 12
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#server.close()	# just in case
server.bind((host,port))
server.listen(1)
input = [server,sys.stdin]
running = 1
connected = 0
print "Ready"
while running:
	usrDisconnected()
	serData = ser.read(13)
	if len(serData) > 0:
		str1 = ''
		print "PIC DATA: Start Loop "
		for m in serData:
			str1 += hex(ord(m))
			str1 += ' '
		print str1 
	inputready,outputready,exceptready = select.select(input,[],[])
	for s in inputready:
		if s == server:
			# handle the server socket
			client, address = server.accept()
			input.append(client)
			#ser.write("User Connected")
			print "User Connected"
##			usrConnected()
		elif s == sys.stdin:
			# handle standard input
			junk = sys.stdin.readline()
			running = 0

		else:
			# handle all other sockets
			data = s.recv(size)
			if data:
				print "Network Data: "
				str2 = ''
				for m in data:
					str2 += hex(ord(m))
					str2 += ' '
				print str2 
				ser.write(data)
			else:
				s.close()
				input.remove(s)
				#ser.write('User Disconnected')
				print "User Disconnected"
				stp()			## Stop the robot if the user disconnects
				#ser.close()
##				usrDisconnected()
	serData = ser.read(13)
	if len(serData) > 0:
		str1 = ''
		print "PIC Data: END LOOP "
		for m in serData:
			str1 += hex(ord(m))
			str1 += ' '
		print str1 
server.close()
ser.close()
